#include "Motor.h"
#include "PWM.h"

/// @brief 电机初始化
void Motor_Init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	PWMABC_Init();
	PWMD_Init();
}

/// @brief 设置电机A的速度
/// @param Speed 正数向前，负数向后
void Motor_SetMotorA_Speed(int8_t Speed)
{
	GPIO_SetBits(MOTOR_AB_STEP);
	if (Speed >= 0)
	{
		GPIO_SetBits(MOTOR_A1);
		GPIO_ResetBits(MOTOR_A2);
		TIM_SetCompare1(TIM2, Speed);
	}
	else if (Speed < 0)
	{
		GPIO_ResetBits(MOTOR_A1);
		GPIO_SetBits(MOTOR_A2);
		TIM_SetCompare1(TIM2, -Speed);
	}
}

/// @brief 设置电机B的速度
/// @param Speed 正数向前，负数向后
void Motor_SetMotorB_Speed(int8_t Speed)
{
	GPIO_SetBits(MOTOR_AB_STEP);
	if (Speed >= 0)
	{
		GPIO_SetBits(MOTOR_B1);
		GPIO_ResetBits(MOTOR_B2);
		TIM_SetCompare2(TIM2, Speed);
	}
	else if (Speed < 0)
	{
		GPIO_ResetBits(MOTOR_B1);
		GPIO_SetBits(MOTOR_B2);
		TIM_SetCompare2(TIM2, -Speed);
	}
}

/// @brief 设置电机C的速度
/// @param Speed 正数向前，负数向后
void Motor_SetMotorC_Speed(int8_t Speed)
{
	GPIO_SetBits(MOTOR_CD_STEP);
	if (Speed >= 0)
	{
		TIM_SetCompare3(TIM2, Speed);
		TIM_SetCompare4(TIM2, 0);
	}
	else
	{
		TIM_SetCompare3(TIM2, 0);
		TIM_SetCompare4(TIM2, -Speed);
	}
}

/// @brief 设置电机D的速度
/// @param Speed 正数向前，负数向后
void Motor_SetMotorD_Speed(int8_t Speed)
{
	GPIO_SetBits(MOTOR_CD_STEP);
	if (Speed >= 0)
	{

		TIM_SetCompare1(TIM4, Speed);
		TIM_SetCompare2(TIM4, 0);
	}
	else
	{
		TIM_SetCompare1(TIM4, 0);
		TIM_SetCompare2(TIM4, -Speed);
	}
}

/// @brief 设置电机CD停止
void MotorCD_Stop()
{
	GPIO_ResetBits(MOTOR_CD_STEP);
	TIM_SetCompare3(TIM2, 100);
	TIM_SetCompare4(TIM2, 100);
	TIM_SetCompare1(TIM4, 100);
	TIM_SetCompare2(TIM4, 100);
}

/// @brief 设置全部电机停止
void Motor_Stop()
{
	TIM_SetCompare1(TIM2, 0);
	TIM_SetCompare2(TIM2, 0);
	GPIO_ResetBits(GPIOA, GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8);
	GPIO_ResetBits(MOTOR_AB_STEP);
	MotorCD_Stop();
}
